#ifndef _RMRPIDIO_H
#define _RMRPIDIO_H

#include "WPILib.h"
#include "RMRIO.h"

/*
 * RMRPIDIO.h
 * 
 * RMR classes for PID input/output
 * 
 * There are 3 classes in this file:
 * 
 * PIDEncoder (an Encoder which is a PIDSource for position/velocity)
 * PIDAbsoluteEncoder (an absolute angle encoder which is a PIDSource for postion)
 * PIDJaguar (a Jaguar which is a PIDOutput for velocity)
 * PIDOuputDuplicator (two PIDOutputs in one)
 * 
 */


class PIDEncoder : public Encoder, public PIDSource
{
public:
	// constructors
	
	// constructor, passed aSlot, aChannel, bSlot, bChannel, [reverseDirection]
	PIDEncoder(UINT32 aSlot, UINT32 aChannel, UINT32 bSlot, UINT32 bChannel, bool reverseDirection=false, CounterBase::EncodingType);
	
	// constructor, passed aChannel, bChannel, [reverseDirection]
	PIDEncoder(UINT32 aChannel, UINT32 bChannel, bool reverseDirection=false, CounterBase::EncodingType);
		
	// adjust scaling -- what we multiply the # of encoder clicks (from the Get() method) by to get revolutions
	void SetScaling(float scaling);
	float GetScaling(void);
	
	// interface to PIDSource
	// if we are in speed mode (user called SetSpeedMode) return the current wheel speed
	// if we are in position mode (user caled SetPositionMode) return the current wheel position
	double PIDGet();
	

	// returns position in # of wheel revolutions
	float GetSpeed();
	
	// returns wheel speed in # of wheel revolutions per second
	float GetPosition();
	
	// is the encoder enabled?
	bool IsEnabled();
	
	// Defaults for configuration options
	// set whenever an object is created
	void SetDefaults(void);
	
	
	
	// override Encoder functions
	// Start the encoder listening for clicks
	void Start();
	// reset the count of the encoder to zero
	void Reset();
	// stop counting clicks
	void Stop();
	
	// Tell PIDEncoder that PIDGet() should return wheel speed
	void SetSpeedMode();
	
	// Tell PIDEncoder that PIDGet() should return wheel position
	void SetPositionMode();
	
	// is the encoder in speed mode?
	bool IsSpeedMode();
	
	bool IsReversed();
	
	// two choices for mode: kSpeed and kPosition
	typedef enum
	{
		kSpeed = 0,
		kPosition = 1,
	} ModeType;
	
	
private:
	float m_scaling;
	
	INT32 m_last_x;
	float m_last_t;
	
	bool m_reversed;
	
	ModeType m_mode;
	
	bool m_enabled; // is the encoder enabled?
	
//	Timer m_timer;
};





class PIDAbsoluteEncoder : public PIDSource
{
public:
	PIDAbsoluteEncoder(UINT32 slot, UINT32 channel);
	PIDAbsoluteEncoder(UINT32 slot);
	
	// Destructor needed to avoid weird compile error
	virtual ~PIDAbsoluteEncoder();
	
	// The PIDSource::PIDGet method
	// return the angle, in terms of -1 is left and +1 is right
	double PIDGet();
	
	
	//Calibrates the min, max, and direction parameters based on the limit switches
	// UNTESTED!!! USE AT YOUR OWN RISK!!!
	// this probably will break the watchdog, confuse the pid, etc...
	void AutoCalibrate(DigitalInput &left_limit_switch, DigitalInput &right_limit_switch, Jaguar &turn_motor, Watchdog &watchdog);

	// Set defaults for member variables
	void Init();
	
	/*
	 * Member variable getters and setters...
	 */
	void SetMin(int min);
	int GetMin();
	
	void SetMax(int max);
	int GetMax();
	
	void SetBackwards(bool backwards);
	bool IsBackwards();
	
	void SetOffset(int offset);
	int GetOffset();
	

	//Return pulse width of the absolute encoder signal
	float GetPulseWidth();
	
	
	/*
	 * Constants
	 */
	// speed (range = -1 .. 1)
	static const float STEER_CALIBRATE_SPEED = 0.1;
	// time in seconds for autocalibration (after this the watchdog starves and the robot dies)
	static const float MAX_TIME_FOR_AUTOCALIBRATE = 30.0;
	
private:
	Counter abs_encoder_count; // counter object to listen on the encder and count the width of the pulses
	int m_min; // angles below this are mapped to -1
	int m_max; // angles above this are mapped to +1
	bool m_backwards; // true = reversed, false = normal
	
	// this has been replaced by m_offset
	int m_crossover; // crossover point (since the absolute encoder wraps from 1024 to 0)
	
	int m_offset;


};





class PIDJaguar : public Jaguar
{
public:
	// constructor, passed channel
	PIDJaguar(UINT32 channel);
	
	// constructor, passed slot and channel
	PIDJaguar(UINT32 slot, UINT32 channel);
	
	
	// Defaults for configuration options
	// set whenever an object is created
	void SetDefaults(void);
	
	
	// Set the jaguar to the specefied output (in range -1 .. 1)
	void PIDWrite(float output);
	
	
	// if inverted is true, the motor will be driven backwards
	void SetInverted(bool inverted);
	
	// returns true if the motor direction is inverted
	bool GetInverted();

	// returns -1 if inverted, +1 otherwise
	INT32 GetDirection();
	
	
	// switch on and off the acceleration limiting
	void EnableLimitAcceleration(void);
	void DisableLimitAcceleration(void);
	
	// max change in wheel speed [rev/s] between two update periods [default 0.05 s]
	static const float MAX_WHEEL_ACCEL = .2;

private:
	
	int m_direction; // -1 = inverted, +1 = normal
	float m_max_wheel_acceleration;
	float m_last_speed;
	bool m_limit_acceleration_enabled;
	
	// send speed in, get acceleration-limited speed out
	float LimitAcceleration(float speed);
};



/*
 * PIDOutputDuplicator
 * 
 * control two PIDOutputs in one interface
 * compatible with PIDController
 * 
 */

class PIDOutputDuplicator : public PIDOutput
{
public:
	PIDOutputDuplicator(PIDOutput *outputA, PIDOutput *outputB);
	
	void PIDWrite(float output);
	
private:
	PIDOutput *m_outputA;
	PIDOutput *m_outputB;
};





#endif

